#pragma once
#include <ros/ros.h>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <Eigen/Geometry>
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <iostream>
#include <fstream>
#include <chrono>
#include <deque>
#include "RobustError.hpp"
#include "serial_com/comm.h"
#include "aim_deps/Paths.hpp"
#include "readParam.hpp"

#define DEQUE_SIZE 16
#define ACC_BUF_MAX_SIZE 4      // 4帧最多50ms 所以可以认为加速度在这个时间内恒定
// #define OUTPUT_FILE             // 保存曲线信息

// 欧拉角转四元数 RPY 定义
static Eigen::Quaterniond angle2Quat(float r, float p, float y, bool is_rad = false){
    if (is_rad == false){
        r *= 0.0174533;       // pi / 180 = 0.0174533
        p *= 0.0174533;
        y *= 0.0174533;     
    }
    return Eigen::Quaterniond (
        cosf(r / 2) * cosf(p / 2) * cosf(y / 2) + sinf(r / 2) * sinf(p / 2) * sinf(y / 2),  // w
        sinf(r / 2) * cosf(p / 2) * cosf(y / 2) - cosf(r / 2) * sinf(p / 2) * sinf(y / 2),  // x
        cosf(r / 2) * sinf(p / 2) * cosf(y / 2) + sinf(r / 2) * cosf(p / 2) * sinf(y / 2),  // y
        cosf(r / 2) * cosf(p / 2) * sinf(y / 2) - sinf(r / 2) * sinf(p / 2) * cosf(y / 2)   // z
    );
}

static void project2World(
    const cv::Point3f& t_cam,
    float pit, float yaw,
    Eigen::Vector3d& pw,
    Eigen::Quaterniond& c2w
){
    c2w = angle2Quat(0.0, pit, yaw);
    Eigen::Vector3d cam_t(t_cam.x, t_cam.y, t_cam.z);
    pw = c2w * cam_t;
}

enum PredictMode {
    FULL_POST_PROCESS       = 0,        // 全预测
    NO_POST_PROCESS         = 1,        // 预测后的处理全部关闭（比如超调保护，静止目标预测削减等等）
    NO_OVERSHOOT_PROTECT    = 2,        // 超调削减保护关闭（减速的时候削减预测）
    NO_STATIC_SUPPRESS      = 3,        // 静止目标削减预测关闭
};

class Predict {
public:
    Predict();
    ~Predict();
public:
    bool isInit() const {
        return this->init;
    }

    void setK(float _k) {
        this->air_k = _k;
    }

    /// @brief 输入当前敌方位姿，根据pitch, yaw进行投影，在世界坐标系下进行预测，预测结果反投影回到相机坐标系，便于电控接收
    /// @return true 预测成功,使用预测值,否则使用装甲板
    bool translatePredict(const cv::Point3f& t_cam, const serial_com::comm& msg, Eigen::Vector3d& cam_p);

    /// @brief 预测绘制
    void drawProjected(cv::Mat& src, const Eigen::Vector3d& tar, const Eigen::Vector3d& pre);

    /// @brief 切换目标需要进行reset
    void reset();

    void initWithROS(ros::NodeHandle& nh);

    void updateTime();
private:
    /// 根据当前值计算观测值，需要计时
    void calcObvserved(const Eigen::Vector3d& pw, Vector6d& obs, double dt, double lambda = 0.5);

    /// 计算状态转移矩阵
    void calcStateTransit(double dt);

    void noiseDEstimate(const Vector6d& obs);
    
    /// 对结果进行后续处理
    /// 1. 减速预测削减
    /// 2. 稳态抖动减小
    void postProcess(Eigen::Vector3d& predict);

    
private:
    Matrix6d A;             // 状态转移
    Matrix6d P;             // 状态转移协方差
    Matrix6d Q;             // 状态转移误差
    Matrix6d R;             // 观测误差
    Vector6d state_post;    // 后验状态
    Vector6d old_obs;
    std::chrono::_V2::system_clock::time_point saved_time_point;
    Eigen::Matrix3d this_K;

    size_t inov_cnt;
    std::deque<double> innovation[6];           // 新息
    std::deque<double> acc_buff[2];             // 两轴加速度缓冲区
    ceres::Solver::Options state_opts;
    std::deque<double> median[4];    // 中值滤波
    PredictMode predict_mode;                 // 预测模式

    bool init;
    double air_k;
    double coeff_vx;
    double coeff_vz;
    double coeff_ax;
    double coeff_az;
    double speed_thresh;
    double acc_thresh;

    #ifdef OUTPUT_FILE
    std::fstream file;
    #endif
};